#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S2,     HTPB,           sensorI2CCustom9V)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//
#include "drivers/HTPB-driver.h"
#include "basic.h"

#include "JoystickDriver.c"
#include "ballhoop definitions.h"
task main ()
{
servo[servo1] = CLAMP_MIDDLE; //put clamp in starting postition
servo[servo4] = BALLHOOP_HOME;// put bowling ball holder in starting position, for 1 crate only, BALLHOOP_START for 2 crates
servo[servo2] = 128;
servo[servo3] = 128;
waitForStart();
//goXCM(735,100);
goXCM (39, -50, .003);  // backward (clamp dir) 40 in down ramp 37,-50,0.006  39, -50, .003
wait10Msec(100);
turnXDg(250, -100);  // turn 30 degrees
wait10Msec(100);
clamp(CLAMP_OPEN);
//goXCM(1.5, 90, .96);//go backward 3 inches.
//StartTask (cratelift);
//if grabbing crates with the raquet balls on top
//setcratelift (CRATE1);
//cltarget = CRATE1;
//while( cltargetreached == false)
//{}
//clamp(CLAMP_HOME);
//wait10Msec(100);
//goXCM (4, -75, .09); //shake balls off of crate stack
//wait10Msec(30);
//goXCM(1, 85, .96);//go backward a bit to free any obstruction
//setcratelift (SPIN);
//goXCM(3, 70, .8); //go backward to free clamped crate from bottom one
//wait10Msec(25);
//servo[servo3] = SPINBACKWARD;
//wait10Msec(40);//wait 1/4 of a second
//servo[servo3] = 128;
//setcratelift (CLHOME);
//clamp(CLAMP_OPEN);
}
/*cltarget = CRATE2;
cltarget = SPIN;
servo[servo3] = SPINBACKWARD;
wait10Msec();
cltarget = CLHOME;
clamp(CLAMP_OPEN);

//if going for middle crate
cltarget = CRATE1;
clamp(CLAMP_HOME);
cltarget  = SPIN;
servo[servo3] = SPINBACKWARD;
wait10Msec();
cltarget = CLHOME;

}

*/
